﻿/// stereo calibration
///
#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

#include "CvCalibCommon.h"
#include "CvCalib3D.h"

using namespace cv;
using namespace std;


static string s_imageFolder("D:/sai/opencv/images/stereo_example/");
static string s_outputFolder("D:/sai/opencv/tmp/");


static void saveXYZ(string filename, const Mat& mat)
{
    const double max_z = 1.0e4;
    FILE* fp = fopen(filename.c_str(), "wt");
    for(int y = 0; y < mat.rows; y++)
    {
        for(int x = 0; x < mat.cols; x++)
        {
            Vec3f point = mat.at<Vec3f>(y, x);
            if(fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "[%d, %d]: %f %f %f\n", x, y , point[0], point[1], point[2]);
        }
    }
    fclose(fp);
}

static void readCameraParamersFromFile(Mat &M1, Mat &D1, Mat &M2, Mat &D2,
                               Mat &R, Mat &T)
{
    FileStorage fs("D:/sai/opencv/tmp/parameters.yml", FileStorage::READ);

    fs["M1"] >> M1;
    fs["D1"] >> D1;
    fs["M2"] >> M2;
    fs["D2"] >> D2;

//    cout << "M1 = " << M1 << endl;
//    cout << "D1 = " << D1 << endl;
//    cout << "M2 = " << M2 << endl;
//    cout << "D2 = " << D2 << endl;

    fs["R"] >> R;
    fs["T"] >> T;

//    cout << "R = " << R << endl;
//    cout << "T = " << T << endl;
}


// 3D坐标计算
static void stereoCal()
{
    //FileStorage fs("D:/sai/opencv/tmp/parameters.yml", FileStorage::READ);

    Size imageSize = Size(640,480);

    Mat M1, D1, M2, D2;
    Mat R, T;
    readCameraParamersFromFile(M1, D1, M2, D2, R, T);

    cout << "M1 = " << M1 << endl;
    cout << "D1 = " << D1 << endl;
    cout << "M2 = " << M2 << endl;
    cout << "D2 = " << D2 << endl;

    cout << "R = " << R << endl;
    cout << "T = " << T << endl;

    Mat R1, P1, R2, P2, Q;
    Rect roi1, roi2;

    stereoRectify( M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, Q, 0, 1, imageSize, &roi1, &roi2 );

    cout << "R1 = " << R1 << endl;
    cout << "R2 = " << R2 << endl;
    cout << "P1 = " << P1 << endl;
    cout << "P2 = " << P2 << endl;
    cout << "Q = " << Q << endl;

    // BM
    int SADWindowSize, numberOfDisparities;
    SADWindowSize = 5;
    numberOfDisparities = ((imageSize.width/8) + 15) & -16;

    Ptr<StereoBM> bm = StereoBM::create(16,9);

    Mat img1 = imread(s_imageFolder + "left01.jpg", IMREAD_GRAYSCALE);
    Mat img2 = imread(s_imageFolder + "right01.jpg", IMREAD_GRAYSCALE);

    // rectify images
    {
        Mat map11, map12, map21, map22;
        initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, map12);
        initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, map22);

        Mat img1r, img2r;
        remap(img1, img1r, map11, map12, INTER_LINEAR);
        remap(img2, img2r, map21, map22, INTER_LINEAR);

        imwrite(s_outputFolder + "left01_r.jpg", img1r);
        imwrite(s_outputFolder + "right01_r.jpg", img2r);
        img1 = img1r;
        img2 = img2r;
    }

    bm->setROI1(roi1);
    bm->setROI2(roi2);
    bm->setPreFilterCap(31);
    bm->setBlockSize(5);
    bm->setMinDisparity(SADWindowSize);
    bm->setNumDisparities(numberOfDisparities);
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(15);
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(1);

    Mat disp, disp8;
    float disparity_multiplier = 1.0f;

    bm->compute(img1, img2, disp);
    if (disp.type() == CV_16S) {
        disparity_multiplier = 16.0f;
        cout << "type = " << disp.type() << endl;
        cout << "D1 = " << disp.at<short>(0, 0) << endl;
    }

    cout <<("storing the point cloud...") << endl;
    fflush(stdout);
    Mat xyz;
    Mat floatDisp;
    disp.convertTo(floatDisp, CV_32F, 1.0f / disparity_multiplier);
    cout << "floatDisp = " << floatDisp.size() << endl;
    reprojectImageTo3D(floatDisp, xyz, Q, true);
    saveXYZ(s_outputFolder + "points.txt", xyz);
    printf("\n");
}

static void singleCalibration(Size &imageSize, Size &boardSize, Size &squareSize,
                              string &inputFolder, string &outputFolder,
                              string &fileNameFormatL, string &fileNameFormatR,
                              vector<string> &imageList,
                              Mat &cameraMatrix, Mat &distCoeffs)
{

    int i, j, k;

    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      inputFolder, outputFolder,
                      fileNameFormatL, fileNameFormatR);

    vector<vector<Point2f> > imagePoints;   // 左右相机角点
    vector<vector<Point3f> > objectPoints;     // 三维坐标
    vector<string> goodImageList;
    int nimages = imageList.size();

    objectPoints.resize(nimages);

    for( i = 0; i < nimages; i++ )
    {
        for( j = 0; j < boardSize.height; j++ )
            for( k = 0; k < boardSize.width; k++ )
                objectPoints[i].push_back(Point3f((float)k*squareSize.width, (float)j*squareSize.height, 0.0));
    }

    // 1. 获取数据
    calib3d.getSingleImagePoints(imagePoints, nimages, imageList, goodImageList);

    // 2. 矫正
    vector<cv::Mat> rvecs;       /* 每幅图像的旋转向量 */
    vector<cv::Mat> tvecs;       /* 每幅图像的平移向量 */

//    double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs,
//                                     rvecs, tvecs, 0);
    int iFixedPoint = -1; // boardSize.width - 1
    //iFixedPoint = boardSize.width - 1;
    vector<Point3f> newObjPoints = objectPoints[0];
    double rms = calibrateCameraRO(objectPoints, imagePoints, imageSize, iFixedPoint,
                                   cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints,
                                   0);
    cout << "rms = " << rms << endl;
    if (iFixedPoint != -1) {
        cout << "New board corners: " << endl;
        cout << newObjPoints[0] << endl;
        cout << newObjPoints[boardSize.width - 1] << endl;
        cout << newObjPoints[boardSize.width * (boardSize.height - 1)] << endl;
        cout << newObjPoints.back() << endl;
    }

    // 3. 对结果进行评价
    vector<float> reprojErrs;
    double totalAvgErr = CvCalibCommon::computeReprojectionErrors(objectPoints, imagePoints,
                 rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
    cout << "per errs = " << Mat(reprojErrs) << endl;
    cout << "err = " << totalAvgErr << endl;

}

/// 演示单独对两个相机进行标定
static void demoSingleCalibration()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(8,5);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    string inputFolder = "D:/sai/opencv/images/aimdata/left/";
    string outputFolder = "D:/sai/opencv/images/aimdata/leftoutput/";
    string fileNameFormatL = "C0_%d.bmp";
    string fileNameFormatR = "C1_%d.bmp";

    FileStorage fs(outputFolder + "camera_params.yml", FileStorage::APPEND);

    Mat cameraMatrixL= Mat::eye(3,3,CV_64F); // 摄像机内参数矩阵
    Mat distCoeffsL = Mat::zeros(1, 5, CV_64F); // 摄像机的5个畸变系数：k1,k2,p1,p2,k3
    vector<string> imageListL;

    int nimages = 15;
    int i;
    for (i= 1; i <= nimages; i++) {
        char buf[30];
        snprintf(buf, 30, fileNameFormatL.c_str(), i);
        imageListL.push_back(buf);
    }

    singleCalibration(imageSize, boardSize, squareSize,
                      inputFolder, outputFolder,
                      fileNameFormatL, fileNameFormatR, imageListL,
                      cameraMatrixL, distCoeffsL);

    cout << "cameraMatrixL = " << cameraMatrixL << endl;
    cout << "distCoeffsL = " << distCoeffsL << endl;
    fs << "M1" << cameraMatrixL;
    fs << "D1" << distCoeffsL;


    ////////////////////////
    string inputFolderR = "D:/sai/opencv/images/aimdata/right/";
    Mat cameraMatrixR = Mat::eye(3,3,CV_64F); // 摄像机内参数矩阵
    Mat distCoeffsR = Mat::zeros(1, 5, CV_64F); // 摄像机的5个畸变系数：k1,k2,p1,p2,k3
    vector<string> imageListR;


    nimages = 16;
    for (i= 1; i <= nimages; i++) {
        char buf[30];
        snprintf(buf, 30, fileNameFormatR.c_str(), i);
        imageListR.push_back(buf);
    }
    singleCalibration(imageSize, boardSize, squareSize,
                      inputFolderR, outputFolder,
                      fileNameFormatL, fileNameFormatR, imageListR,
                      cameraMatrixR, distCoeffsR);

    cout << "cameraMatrixR = " << cameraMatrixR << endl;
    cout << "distCoeffsR = " << distCoeffsR << endl;
    fs << "M2" << cameraMatrixR;
    fs << "D2" << distCoeffsR;

    if(fs.isOpened())
        fs.release();
}

/// 演示如何查找角点数据
static void demoGetDualImagePoints()
{

    vector<vector<Point2f> > imagePoints[2];   // 左右相机角点
    Size imageSize = Size(640,480);
    Size boardSize = Size(9,6);             // 定标板上每行、列的角点数
    int nimages = 14;                       // 左右各14张图

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    vector<string> goodImageList;

    CvCalib3D calib3d;
    calib3d.getDualImagePoints(imagePoints, nimages, goodImageList);
}

static void demoStereoCalib()
{
    Size imageSize = Size(640,480);
    Size boardSize = Size(9,6);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/stereo_example/input/",
                      "D:/sai/opencv/images/stereo_example/output/",
                      "left%02d.jpg", "right%02d.jpg");

    int nimages = 14;
    string paramsFileName = "params";
    calib3d.stereoCalib(nimages, paramsFileName);
}

static void demoStereoCalib1()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(8,5);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/aimdata/test01/",
                      "D:/sai/opencv/images/aimdata/output/",
                      "C0_%d.bmp", "C1_%d.bmp");
    int nimages = 16;
    string paramsFileName = "params";
    calib3d.stereoCalib(nimages, paramsFileName);
}

static void demoStereoCalib2()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(9,6);             // 定标板上每行、列的角点数
    Size squareSize = Size(25,25);
    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/aimdata/test02/",
                      "D:/sai/opencv/images/aimdata/output02/",
                      "C0_%d.bmp", "C1_%d.bmp");
    int nimages = 17;
    string paramsFileName = "params";
    calib3d.stereoCalib(nimages, paramsFileName);

    if(false) {
        vector<vector<Point2f> > imagePoints[2];   // 左右相机角点
        imagePoints[0].resize(nimages);
        imagePoints[1].resize(nimages);
        vector<string> goodImageList;
        calib3d.getDualImagePoints(imagePoints, nimages, goodImageList);
    }
}


static void demoStereoCalib3()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(8,5);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    string inputFolder = "D:/sai/opencv/images/aimdata/test03/";
    string outputFolder = "D:/sai/opencv/images/aimdata/output03/";
    string fileNameFormatL = "C0_%d.bmp";
    string fileNameFormatR = "C1_%d.bmp";
    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      inputFolder, outputFolder, fileNameFormatL,fileNameFormatR);

    int nimages = 16;
    string paramsFileName = "params";

    FileStorage fs(outputFolder + "camera_params.yml", FileStorage::READ);
    Mat M[2],D[2];
    fs["M1"] >> M[0];
    fs["D1"] >> D[0];
    fs["M2"] >> M[1];
    fs["D2"] >> D[1];

    calib3d.stereoCalib(M,D,nimages, paramsFileName);
}

/// 计算一幅图的理想到畸变后的尺寸映射
static void demoMapToDistortedUV()
{
    Mat M1, D1, M2, D2;
    Mat R, T;
    readCameraParamersFromFile(M1, D1, M2, D2, R, T);

    Mat invM1 = M1.inv();

    cout << "M1 = " << M1 << endl;
    cout << "invM1 = " << invM1 << endl;
    Size imageSize = Size(640,480);

    // 将计算结果保存到文件中
    string filename = s_outputFolder + "uv_map.txt";
    FILE* fp = fopen(filename.c_str(), "wt");
    int i, j;
    for (i=0; i< imageSize.height; i++) {
        for (j=0; j< imageSize.width;j++) {
            Point2f uv(i+1,j+1);
            Point2f distUv = CvCalibCommon::uvToDistortedUV(uv, M1, invM1, D1);
            // cout << "map: " << uv << " to " << distUv << endl;
            fprintf(fp, "[%f, %f] to [%f, %f]\n", uv.x, uv.y , distUv.x, distUv.y);
        }
    }
    fclose(fp);
}

/// 三维坐标和二维坐标互相转化验证
static void demoPoint3DtoUV()
{
    Mat M1, D1, M2, D2;
    Mat R, T;
    readCameraParamersFromFile(M1, D1, M2, D2, R, T);

    cout << "M1 = " << M1 << endl;
    cout << "D1 = " << D1 << endl;
    cout << "M2 = " << M2 << endl;
    cout << "D2 = " << D2 << endl;

    cout << "R = " << R << endl;
    cout << "T = " << T << endl;

    // 计算第一个角点的世界坐标
    Point3f worldPoint(-89.80852796443482, -129.3784012444383, 477.0585225098047);
    //Mat workdPointMat = Mat(worldPoint);
    //workdPointMat.reshape(3, 1);

    cout << "type === " << R.type() << endl;
    Mat R1 = Mat::eye(3,3, CV_64FC1);
    Mat T1 =Mat(3,1,CV_64FC1,Scalar::all(0));
    cout << "R1 = " << R1 << endl;
    cout << "T1 = " << T1 << endl;

    // 计算理想的左右像素点
    Point2f leftPoint = CvCalibCommon::xyz2uv(worldPoint, M1, R1, T1, D1, false);
    Point2f rightPoint = CvCalibCommon::xyz2uv(worldPoint, M2, R, T, D2, false);
    cout << "leftPoint = " << leftPoint << endl;
    cout << "rightPoint = " << rightPoint << endl;

    // 反算三维空间点
    Point3f pointRetW = CvCalibCommon::uv2xyz(leftPoint, rightPoint, M1, R1, T1,
                                              M2, R, T);
    cout << "空间坐标: " << pointRetW << endl;
}

/// 将角点坐标转成3D坐标
static void demoCornersTo3D()
{
    // 0. 读取参数
    Mat M1, D1, M2, D2;  // 内参和畸变
    Mat R, T;            // 第一个和第二个之间的旋转矩阵
    readCameraParamersFromFile(M1, D1, M2, D2, R, T);

    // 定义 单位旋转矩阵和0平移矩阵
    Mat R0 = Mat::eye(3,3, CV_64FC1);
    Mat T0 =Mat(3,1,CV_64FC1,Scalar::all(0));

    int i;

    // 1. 读取原始图像
    Size imageSize(1280, 720);
    Size boardSize = Size(8,5);        // 定标板上每行、列的角点数
    Mat img1 = imread(s_imageFolder + "C0_1.bmp", IMREAD_GRAYSCALE);
    Mat img2 = imread(s_imageFolder + "C1_1.bmp", IMREAD_GRAYSCALE);


    //Mat P1 = getOptimalNewCameraMatrix(M1, D1, imageSize, 0, imageSize, 0);
    //cout << "P1 = " << P1 << endl;

    // 2. 矫正图像
    Mat map1x, map1y, map2x, map2y;
    // initUndistortRectifyMap(M1, D1, R, M1, imageSize, CV_32FC1, map1x, map1y);
    initUndistortRectifyMap(M1, D1, R0, M1, imageSize, CV_16SC2, map1x, map1y);
    initUndistortRectifyMap(M2, D2, R0, M2, imageSize, CV_16SC2, map2x, map2y);

    Mat img1R = img1.clone();
    Mat img2R = img2.clone();
    cv::remap( img1, img1R, map1x, map1y, INTER_LINEAR);
    cv::remap( img2, img2R, map2x, map2y, INTER_LINEAR);

    // 3. 记录矫正的结果
    imwrite(s_outputFolder + "left01_r.jpg", img1R);
    imwrite(s_outputFolder + "right01_r.jpg", img2R);


    // 4. 获取角点
    vector<Point2f> corners1, corners1R;            // 缓存每幅图像上检测到的角点
    vector<Point2f> corners2, corners2R;            // 缓存每幅图像上检测到的角点

    bool found1 = CvCalibCommon::findChessboardCorners(img1, boardSize, corners1);
    bool found2 = CvCalibCommon::findChessboardCorners(img1R, boardSize, corners1R);
    if (!found1 || !found2) {
        cout << "Not found corners!" << endl;
        return;
    }

    found1 = CvCalibCommon::findChessboardCorners(img2, boardSize, corners2);
    found2 = CvCalibCommon::findChessboardCorners(img2R, boardSize, corners2R);
    if (!found1 || !found2) {
        cout << "Not found corners!" << endl;
        return;
    }

    // 5. 计算三维坐标
    vector<Point3f> pointsWorld;
    for (i=0; i< corners1R.size();i++) {
        Point3f worldPoint = CvCalibCommon::uv2xyz(corners1R[i], corners2R[i], M1, R0, T0,
                                   M2, R, T);
        //cout << i << ": " << corners1[i] << ", " << corners2R[i] ;
        //cout << " worldPoint: " << worldPoint << endl;
        pointsWorld.push_back(worldPoint);
    }

    // save to file
    {
        string filename = s_outputFolder + "corners_map.txt";
        ofstream fpoint(filename);
        // FILE* fp = fopen(filename.c_str(), "wt");
        for (i=0; i< corners1.size(); i++) {
//            fprintf(fp, "[%f, %f], [%f, %f], [%f, %f], [%f, %f]\n",
//                    corners1[i].x, corners1[i].y , corners1R[i].x,  corners1R[i].y,
//                    corners2[i].x, corners2[i].y , corners2R[i].x,  corners2R[i].y
//                    );
            fpoint << i+1 << ": " << corners1[i] << ", " << corners2R[i] ;
            fpoint << " worldPoint: " << pointsWorld[i] << endl;
        }
        fpoint.close();
        //fclose(fp);
    }

    // 6. 计算两两点的距离
    cout << "distances:";
    for (i=0; i < pointsWorld.size() - 10;i++) {
        Point3f point1 = pointsWorld[i+1] - pointsWorld[i];
        Point3f point2 = pointsWorld[i+9] - pointsWorld[i];
        double d1 = sqrt(point1.ddot(point1));
        double d2 = sqrt(point2.ddot(point2));
        if (i%9 == 0) {
            cout << endl;
            cout << i+1 << ": ";
        }
        cout << " [" << d1 << "," << d2 << "]";
    }
    cout << endl;


    // 7. 对计算结果进行评价
    {
        vector<Point2f>  imagePoints1, imagePoints2; // 保存重新计算得到的投影点
        double err1 = 0.0, err2 = 0.0;                            // 每幅图像的平均误差
        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        projectPoints(pointsWorld, R0, T0, M1, D1, imagePoints1);
        projectPoints(pointsWorld, R, T, M2, D2, imagePoints2);
        Mat corners1Mat = Mat(1,pointsWorld.size(),CV_32FC2);
        Mat corners2Mat = Mat(1,pointsWorld.size(),CV_32FC2);
        Mat imagePoints1mat = Mat(1,imagePoints1.size(), CV_32FC2);
        Mat imagePoints2mat = Mat(1,imagePoints2.size(), CV_32FC2);
        for (size_t j = 0 ; j != pointsWorld.size(); j++)
        {
            corners1Mat.at<Vec2f>(0,j) = Vec2f(corners1[j].x, corners1[j].y);
            corners2Mat.at<Vec2f>(0,j) = Vec2f(corners2[j].x, corners2[j].y);

            imagePoints1mat.at<Vec2f>(0,j) = Vec2f(imagePoints1[j].x, imagePoints1[j].y);
            imagePoints2mat.at<Vec2f>(0,j) = Vec2f(imagePoints2[j].x, imagePoints2[j].y);
        }
        err1 = norm(imagePoints1mat, corners1Mat, NORM_L2);
        err2 = norm(imagePoints2mat, corners2Mat, NORM_L2);
        cout << "imagePoints1mat = " << imagePoints1mat << endl;
        cout << "corners1Mat = " << corners1Mat << endl;
        cout << "imagePoints2mat = " << imagePoints2mat << endl;
        cout << "corners2Mat = " << corners2Mat << endl;
        cout << "img1 err = " << err1 << endl;
        cout << "img2 err = " << err2 << endl;
    }

}


/// 将角点坐标转成3D坐标
static void demoCornersTo3D0()
{
    Size imageSize = Size(640,480);
    Size boardSize = Size(9,6);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    string fileNameFormatL = "left%02d.jpg";
    string fileNameFormatR = "right%02d.jpg";
    int nimages = 14;

    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/stereo_example/input/",
                      "D:/sai/opencv/images/stereo_example/output/",
                      fileNameFormatL, fileNameFormatR);


    // 0. 读取参数
    calib3d.readParamsFromFiles("params.yml");

    // 1. 读取原始图像       // 定标板上每行、列的角点数
    for (int i=1;i<=nimages;i++) {
        cout << ">>> compute " << i << endl;
        char buf[20];
        snprintf(buf, 20, fileNameFormatL.c_str(), i);
        Mat img1 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);
        snprintf(buf, 20, fileNameFormatR.c_str(), i);
        Mat img2 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);

        double err1 = 0.0, err2 = 0.0;
        calib3d.verifyImagePointsTo3D(img1, img2, "points1.txt", err1, err2);
    }
}

/// 将角点坐标转成3D坐标
static void demoCornersTo3D1()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(8,5);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    string fileNameFormatL = "C0_%d.bmp";
    string fileNameFormatR = "C1_%d.bmp";

    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/aimdata/test01/",
                      "D:/sai/opencv/images/aimdata/output/",
                      fileNameFormatL, fileNameFormatR);

    // 0. 读取参数
    calib3d.readParamsFromFiles("params.yml");

    // 1. 读取原始图像       // 定标板上每行、列的角点数
    for (int i=1;i<=16;i++) {
        cout << ">>> compute " << i << endl;
        if (i== 7) {continue;}
        char buf[20];
        snprintf(buf, 20, fileNameFormatL.c_str(), i);
        Mat img1 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);
        snprintf(buf, 20, fileNameFormatR.c_str(), i);
        Mat img2 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);

        double err1 = 0.0, err2 = 0.0;
        calib3d.verifyImagePointsTo3D(img1, img2, "points1.txt", err1, err2);
    }
}

/// 将角点坐标转成3D坐标
static void demoCornersTo3D2()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(9,6);             // 定标板上每行、列的角点数
    Size squareSize = Size(25,25);
    string fileNameFormatL = "C0_%d.bmp";
    string fileNameFormatR = "C1_%d.bmp";

    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/aimdata/test02/",
                      "D:/sai/opencv/images/aimdata/output02/",
                      fileNameFormatL, fileNameFormatR);

    // 0. 读取参数
    calib3d.readParamsFromFiles("params.yml");

    // 1. 读取原始图像       // 定标板上每行、列的角点数
    for (int i=1;i<=17;i++) {
        cout << ">>> compute " << i << endl;
        char buf[20];
        snprintf(buf, 20, fileNameFormatL.c_str(), i);
        Mat img1 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);
        snprintf(buf, 20, fileNameFormatR.c_str(), i);
        Mat img2 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);

        double err1 = 0.0, err2 = 0.0;
        calib3d.verifyImagePointsTo3D(img1, img2, "points1.txt", err1, err2);
    }
}

/// 将角点坐标转成3D坐标
static void demoCornersTo3D3()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(8,5);             // 定标板上每行、列的角点数
    Size squareSize = Size(30,30);
    string inputFolder = "D:/sai/opencv/images/aimdata/test03/";
    string outputFolder = "D:/sai/opencv/images/aimdata/output03/";
    string fileNameFormatL = "C0_%d.bmp";
    string fileNameFormatR = "C1_%d.bmp";

    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      inputFolder, outputFolder, fileNameFormatL,fileNameFormatR);

    // 0. 读取参数
    calib3d.readParamsFromFiles("params.yml");
    {
        Mat img1 = imread("D:/Apps/Aimooe/AimToolbox/DualImage/C0_3.bmp", IMREAD_GRAYSCALE);
        Mat img2 = imread("D:/Apps/Aimooe/AimToolbox/DualImage/C1_3.bmp", IMREAD_GRAYSCALE);
        double err1 = 0.0, err2 = 0.0;
        calib3d.verifyImagePointsTo3D(img1, img2, "pointsResult3.txt", err1, err2);
    }

    return;

    // 1. 读取原始图像       // 定标板上每行、列的角点数
    for (int i=1;i<=16;i++) {
        cout << ">>> compute " << i << endl;
        char buf[20];
        snprintf(buf, 20, fileNameFormatL.c_str(), i);
        Mat img1 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);
        snprintf(buf, 20, fileNameFormatR.c_str(), i);
        Mat img2 = imread(calib3d.m_inputFolder + buf, IMREAD_GRAYSCALE);

        double err1 = 0.0, err2 = 0.0;
        calib3d.verifyImagePointsTo3D(img1, img2, "points1.txt", err1, err2);
    }
}

static void demoUvto3D()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(9,6);             // 定标板上每行、列的角点数
    Size squareSize = Size(25,25);
    string fileNameFormatL = "C0_%d.bmp";
    string fileNameFormatR = "C1_%d.bmp";

    CvCalib3D calib3d(imageSize, boardSize, squareSize,
                      "D:/sai/opencv/images/aimdata/test03/",
                      "D:/sai/opencv/images/aimdata/output03/",
                      fileNameFormatL, fileNameFormatR);

    // 0. 读取参数
    calib3d.readParamsFromFiles("params.yml");
    cout << "T=" << calib3d.T << endl;

    vector<Point2f> uvsLeft = {
        Point2f(630, 290),
        Point2f(695, 261),
        Point2f(660, 348),
        Point2f(714, 342),

        Point2f(630, 279),
        Point2f(695, 251),
        Point2f(660, 337),
        Point2f(714, 332),
        Point2f(583, 277)
    };
    vector<Point2f> uvsRight = {
        //Point2f(611, 299),
        Point2f(672, 269),
                Point2f(611, 299),
        Point2f(636, 355),
        Point2f(690, 351),

        Point2f(610, 288),
        Point2f(672, 259),
        Point2f(636, 345),
        Point2f(690, 341),
        Point2f(542, 286)
    };
    vector<Point3f> points;

    for (int i=0; i<1;i++) {
        Point3f point = calib3d.uv2xyz(uvsLeft[i], uvsRight[i]);
        points.push_back(point);
        cout<< i+1 <<" w: " << point << endl;
    }

    return;

    for (int i=0; i<3;i++) {
        for (int j=i+1;j<4;j++) {
            Point3f tp = points[j] - points[i];
            double d = sqrt(tp.ddot(tp));
            cout << i+1 << " to " << j+1 << " dist: " << d << endl;
        }
    }

    for (int i=0;i<4;i++) {
        Point3f tp = points[i+4] - points[i];
        double d = sqrt(tp.ddot(tp));
        cout << i+1 << " dist: " << d << endl;
    }
    {
        Point3f tp = points[0] - points[8];
        double d = sqrt(tp.ddot(tp));
        cout << "Z " << " dist: " << d << endl;
    }
}

/// 将两幅棋盘图转成3d坐标
static void demoDualImgTo3D()
{
    Size imageSize = Size(1280,720);
    Size boardSize = Size(8,5);             // 定标板上每行、列的角点数
    Size squareSize = Size(25,25);
    string inputFolder = "D:/sai/opencv/images/aimdata/test03/";
    string outputFolder = "D:/sai/opencv/images/aimdata/output03/";

    FileStorage fs(outputFolder + "params.yml", FileStorage::READ);
    Mat M1, D1, M2, D2;  // 内参和畸变
    Mat R, T;
    fs["M1"] >> M1;
    fs["D1"] >> D1;
    fs["M2"] >> M2;
    fs["D2"] >> D2;
    fs["R"] >> R;
    fs["T"] >> T;

    // 定义 单位旋转矩阵和0平移矩阵
    Mat R0 = Mat::eye(3,3, CV_64FC1);
    Mat T0 =Mat(3,1,CV_64FC1,Scalar::all(0));

    // 1. 读取原始图像, 如果不是灰度，药转成灰度图像
    Mat img1 = imread(inputFolder + "C0_1.bmp", IMREAD_GRAYSCALE);
    Mat img2 = imread(inputFolder + "C1_1.bmp", IMREAD_GRAYSCALE);

    // 2. 矫正图像
    Mat map1x, map1y, map2x, map2y;
    // initUndistortRectifyMap(M1, D1, R, M1, imageSize, CV_32FC1, map1x, map1y);
    initUndistortRectifyMap(M1, D1, R0, M1, imageSize, CV_16SC2, map1x, map1y);
    initUndistortRectifyMap(M2, D2, R0, M2, imageSize, CV_16SC2, map2x, map2y);

    Mat img1R = img1.clone();
    Mat img2R = img2.clone();
    cv::remap( img1, img1R, map1x, map1y, INTER_LINEAR);
    cv::remap( img2, img2R, map2x, map2y, INTER_LINEAR);

    // 3. 记录矫正的结果
    imwrite(outputFolder + "left01_r.jpg", img1R);
    imwrite(outputFolder + "right01_r.jpg", img2R);

    // 4. 获取角点
    vector<Point2f>  corners1R;            // 缓存每幅图像上检测到的角点
    vector<Point2f>  corners2R;            // 缓存每幅图像上检测到的角点

    bool found1 = CvCalibCommon::findChessboardCorners(img1R, boardSize, corners1R);
    bool found2 = CvCalibCommon::findChessboardCorners(img2R, boardSize, corners2R);
    if (!found1 || !found2) {
        cout << "Not found corners!" << endl;
        return;
    }

    // 5. 计算三维坐标
    vector<Point3f> pointsWorld;
    int i;
    for (i=0; i< corners1R.size();i++) {
        Point3f worldPoint = CvCalibCommon::uv2xyz(corners1R[i], corners2R[i], M1, R0, T0,
                                   M2, R, T);
        cout << i+1 << ": " << corners1R[i] << ", " << corners2R[i] ;
        cout << " worldPoint: " << worldPoint << endl;
        pointsWorld.push_back(worldPoint);

        // 反投影回去，如果不是符合要求的点，反映射回去的点和原来的点不匹配
        Point2f leftPoint = CvCalibCommon::xyz2uv(worldPoint, M1, R0, T0, D1, false);
        Point2f rightPoint = CvCalibCommon::xyz2uv(worldPoint, M2, R, T, D2, false);
        double err = norm(Mat(leftPoint), Mat(corners1R[i]), NORM_L2) + norm(Mat(rightPoint), Mat(corners2R[i]), NORM_L2);
        cout << i+1  << ": " << leftPoint << ", " << rightPoint << " , err="<< err << endl ;
    }
}

static void demoOthers()
{
    FileStorage fs("D:/sai/opencv/tmp/parameters.yml", FileStorage::READ);
    Mat M1, D1, M2, D2;  // 内参和畸变
    Mat R, T;
    fs["M1"] >> M1;
    fs["D1"] >> D1;
    fs["M2"] >> M2;
    fs["D2"] >> D2;
    fs["R"] >> R;
    fs["T"] >> T;

    Mat R1, P1, R2, P2, Q;
    Rect roi1, roi2;
    Size imageSize(640, 480);

    stereoRectify( M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, Q, 0, 1, imageSize, &roi1, &roi2 );

    cout << "R1 = " << R1 << endl;
    cout << "R2 = " << R2 << endl;
    cout << "P1 = " << P1 << endl;
    cout << "P2 = " << P2 << endl;
    cout << "Q = " << Q << endl;

//    Mat R1;
//    fs["R1"] >> R1;

    Mat P1_ = M1 * R1;
    cout << "P1_ = " << P1_ << endl;
}

int main(int argc, char const *argv[])
{

    int option = 8;
    //s_imageFolder = "D:/sai/opencv/images/aimdata/test01/";
    //s_outputFolder = "D:/sai/opencv/images/aimdata/out/";

    switch (option) {
    case 0:
    // 单目相机参数标定
        demoSingleCalibration();
        break;
    case 1:
    // 获取双目相机棋盘点
        demoGetDualImagePoints();
        break;
    case 2: // 双目摄像头参数生成
        // 双目矫正参数计算，最后输出到parameters.yml文件中
        demoStereoCalib3();
        break;
    case 3: // 计算立体图像 - 这个不准
        stereoCal();
        // cout << "abc_" + to_string(19) << endl;
        break;

    case 4: // 计算一幅图的理想到畸变后的尺寸映射 uv to distortedUV
        demoMapToDistortedUV();
        break;

    case 5: // 三维坐标到二维坐标转
        demoPoint3DtoUV();
        break;
    case 6: // 一对图的角点转成三维点，并做投影后计算误差。
        demoCornersTo3D3();
        break;

    case 7:
        demoUvto3D();
        break;

    case 8:
        demoDualImgTo3D();
        break;

    case 20:
        demoOthers();
        break;
    default:
        break;
    }
    return 0;
}
